#include "astar.h"
#include "global.h"
#include "modbus.h"
#include "stdio.h"

ASTAR_MAP  astar_map = { 0 };
ASTAR_PATH astar_path = { 0 };
bit astar_map_changed = 1;

enum NODE_DIR
{
    NODE_DIR_TOP,
    NODE_DIR_RIGHT,
    NODE_DIR_BOTTOM,
    NODE_DIR_LEFT,

    NODE_DIR_COUNT,
};

void Astar_Reset_Map(ASTAR_MAP *map)
{
    uchar x, y;

    assert(map->x_node_count < MAX_NODE_COUNT);
    assert(map->y_node_count < MAX_NODE_COUNT);

    for(y=0; y<map->y_node_count; ++y)
    {
        for(x=0; x<map->x_node_count; ++x)
        {
//            Update_WatchDog();
            map->node[x][y].came_from = 0;
            map->node[x][y].next = 0;
            map->node[x][y].state = ASTAR_NODE_STATE_NONE;
            map->node[x][y].g_cost = 0;
            map->node[x][y].f_cost = 0;
        }
    }
    map->closed = 0;
    map->open = 0;
}

ASTAR_NODE xdata * Astar_Select_Node(ASTAR_MAP *map)
{
    ASTAR_NODE *node = map->open;
    if (map->open)
        map->open = map->open->next;
    return node;
}

uchar Astar_H_Cost(char dx, char dy)
{
    if ((int)dx < 0)
        dx = -dx;
    if ((int)dy < 0)
        dy = -dy;
    return dx + dy;
}

void Astar_Add_Open(ASTAR_MAP *map, ASTAR_NODE *node)
{
    ASTAR_NODE *prev = 0;
    ASTAR_NODE *open = map->open;

    while(open)
    {
        if (node->f_cost <= open->f_cost)
            break;
        prev = open;
        open = open->next;
    }

    node->next = open;
    if (prev)
        prev->next = node;
    else
        map->open = node;

    node->state = ASTAR_NODE_STATE_OPEN;
}


// 裁减路径, 将位于一条直线上的中间点删除
void Astar_Prune_Path(ASTAR_NODE *node)
{
    ASTAR_NODE *parent = node->came_from;
    ASTAR_NODE *grandpa = parent ? parent->came_from : 0;

    while(grandpa)
    {
//        Update_WatchDog();
        if ((parent->x == node->x && parent->x == grandpa->x)
                || (parent->y == node->y && parent->y == grandpa->y))
            node->came_from = grandpa;
        else
            node = parent;

        parent = grandpa;
        grandpa = parent->came_from;
    }
}

// 返回指向路径起点的节点N, N->next为下一个节点, ...
ASTAR_NODE xdata * Astar_Construct_Path(ASTAR_NODE *node)
{
    assert(node != 0);

    node->next = 0;

    while(node->came_from)
    {
        node->came_from->next = node;
        node = node->came_from;
    }
    return node;
}

uchar Astar_G_Cost(ASTAR_NODE *from, ASTAR_NODE *to)
{
    uchar cost = from->g_cost + to->cost;
    if (!from->came_from)
        return cost;

    if ((from->came_from->x == from->x && from->x == to->x)
            || (from->came_from->y == from->y && from->y == to->y))
        return cost;

    return cost + from->cost;
}

// 查找i, 使p[i] <= x < p[i+1]
uchar Astar_Find_Index(long *p, uchar n, long x)
{
    uchar i;
    for(i=0; i<n; ++i)
        if (x < p[i+1])
            return i;
    return n-1;
}

bit Astar_Rect_Intersects(ASTAR_MAP *map, ASTAR_NODE *node, RECT *r)
{
    if (r->x0 >= map->border_x[node->x+1]
            || map->border_x[node->x] >= r->x1)
        return 0;

    if (r->y0 >= map->border_y[node->y+1]
            || map->border_y[node->y] >= r->y1)
        return 0;

    return 1;
}

bit Astar_Rect_Is_Valid(RECT *r)
{
    return r->x0 < r->x1 && r->y0 < r->y1;
}

// 将x插入p[0..n]中, 插入前p的元素数为n+1
bit Astar_Insert_Border(long *p, uchar n, long x)
{
    uchar i;

    if (x <= p[0] || x >= p[n])
        return 0;

    i = Astar_Find_Index(p, n, x);
    if (p[i] == x)
        return 0;

    // 此处i>0
    for(; n > i; n--)
        p[n+1] = p[n];
    p[i+1] = x;

    return 1;
}

void Astar_Build_Map()
{
    uchar i, x, y;

    if (!astar_map_changed)
        return;

    astar_map_changed = 0;
    astar_map.x_node_count = 0;
    astar_map.y_node_count = 0;

    if (!Astar_Rect_Is_Valid(&rw_word.range))
        return;

    astar_map.border_x[0] = rw_word.range.x0;
    astar_map.border_x[1] = rw_word.range.x1;
    astar_map.border_y[0] = rw_word.range.y0;
    astar_map.border_y[1] = rw_word.range.y1;
    astar_map.x_node_count = 1;
    astar_map.y_node_count = 1;

    // 划分边界
    for(i=0; i<MAX_BLOCK_COUNT; i++)
    {
        if (!Astar_Rect_Is_Valid(&rw_word.block[i]))
            continue;

        if (Astar_Insert_Border(astar_map.border_x,
                                astar_map.x_node_count,
                                rw_word.block[i].x0))
            ++astar_map.x_node_count;

        if (Astar_Insert_Border(astar_map.border_x,
                                astar_map.x_node_count,
                                rw_word.block[i].x1))
            ++astar_map.x_node_count;

        if (Astar_Insert_Border(astar_map.border_y,
                                astar_map.y_node_count,
                                rw_word.block[i].y0))
            ++astar_map.y_node_count;

        if (Astar_Insert_Border(astar_map.border_y,
                                astar_map.y_node_count,
                                rw_word.block[i].y1))
            ++astar_map.y_node_count;
    }

    assert(astar_map.x_node_count <= MAX_NODE_COUNT);
    assert(astar_map.y_node_count <= MAX_NODE_COUNT);

    // 标记障碍
    for(y=0; y<astar_map.y_node_count; ++y)
    {
        for(x=0; x<astar_map.x_node_count; ++x)
        {
            astar_map.node[x][y].x = x;
            astar_map.node[x][y].y = y;
            astar_map.node[x][y].cost = 1;
            astar_map.node[x][y].wall = 0;

            for(i=0; i<MAX_BLOCK_COUNT; ++i)
            {
//                Update_WatchDog();

                if (!Astar_Rect_Is_Valid(&rw_word.block[i]))
                    continue;

                if (Astar_Rect_Intersects(&astar_map,
                                          &astar_map.node[x][y],
                                          &rw_word.block[i]))
                {
                    astar_map.node[x][y].wall = 1;
                    break;
                }
            }
        }
    }
}

// 查找路径, 返回起始点
ASTAR_NODE xdata *Astar_Find_Path_Node(ASTAR_MAP *map, uchar from_x, uchar from_y, uchar to_x, uchar to_y)
{
    ASTAR_NODE *node, *neighbor;
    uchar i;
    uchar g_cost;

    assert(map != 0);
    assert(from_x < map->x_node_count);
    assert(from_y < map->y_node_count);
    assert(to_x < map->x_node_count);
    assert(to_y < map->y_node_count);

    Astar_Reset_Map(map);

    map->open = &map->node[from_x][from_y];
    map->open->g_cost = 0;
    map->open->f_cost = Astar_H_Cost(from_x - to_x,from_y - to_y);

    while(map->open)
    {
//        Update_WatchDog();
        node = Astar_Select_Node(map);

        if (node->x == to_x && node->y == to_y)
        {
            Astar_Prune_Path(node);
            return map->open = Astar_Construct_Path(node);
        }

        // add to closed list
        node->next = map->closed;
        node->state = ASTAR_NODE_STATE_CLOSE;
        map->closed = node;

        // foreach neighbor
        for(i=0; i<NODE_DIR_COUNT; ++i)
        {
//            Update_WatchDog();
            switch(i)
            {
            case NODE_DIR_TOP:
                if (node->y + 1 >= map->y_node_count)
                    continue;
                neighbor = &map->node[node->x][node->y+1];
                break;

            case NODE_DIR_RIGHT:
                if (node->x + 1 >= map->x_node_count)
                    continue;
                neighbor = &map->node[node->x+1][node->y];
                break;

            case NODE_DIR_BOTTOM:
                if (node->y == 0)
                    continue;
                neighbor = &map->node[node->x][node->y-1];
                break;

            case NODE_DIR_LEFT:
                if (node->x == 0)
                    continue;
                neighbor = &map->node[node->x-1][node->y];
                break;

            default:
                continue;
            }

            if (neighbor->wall || neighbor->state == ASTAR_NODE_STATE_CLOSE)
                continue;

            g_cost = Astar_G_Cost(node, neighbor);
            if (neighbor->state != ASTAR_NODE_STATE_OPEN || g_cost < neighbor->g_cost)
            {
                neighbor->came_from = node;
                neighbor->g_cost = g_cost;
                neighbor->f_cost = g_cost + Astar_H_Cost(neighbor->x - to_x, neighbor->y - to_y);
                if (neighbor->state != ASTAR_NODE_STATE_OPEN)
                    Astar_Add_Open(map, neighbor);
            }
        }
    }

    return 0;
}



bit Astar_Find_Path(long start_x, long start_y, long end_x, long end_y)
{
    uchar from_x, from_y, to_x, to_y;
    ASTAR_NODE *node;
    long x, y;

    Astar_Build_Map();

    astar_path.path_count = 0;
    astar_path.path_index = 0;
    astar_path.path = 0;

    if (astar_map.x_node_count < 2 && astar_map.y_node_count < 2)
        return 0;

    // 查找XY坐标所在区域
    from_x = Astar_Find_Index(astar_map.border_x, astar_map.x_node_count, start_x);
    to_x   = Astar_Find_Index(astar_map.border_x, astar_map.x_node_count, end_x);
    from_y = Astar_Find_Index(astar_map.border_y, astar_map.y_node_count, start_y);
    to_y   = Astar_Find_Index(astar_map.border_y, astar_map.y_node_count, end_y);

    // 在同一方格中
    if (from_x == to_x && from_y == to_y)
        return 0;

    node = Astar_Find_Path_Node(&astar_map, from_x, from_y, to_x, to_y);
    if (node == 0 || node->next == 0)
        return 0;

    astar_path.path = node;

    // 忽略起点和终点
    node = node->next;
    while(node->next)
    {
//        Update_WatchDog();

        assert(node->x < astar_map.x_node_count);
        assert(node->y < astar_map.y_node_count);

        if (node->x == from_x)
        {
            if (to_x < node->x)
                x = astar_map.border_x[node->x];
            else if (to_x > node->x)
                x = astar_map.border_x[node->x + 1];
            else
                x = start_x;
        }
        else if (from_x < node->x)
            x = astar_map.border_x[node->x];
        else
            x = astar_map.border_x[node->x + 1];

        if (node->y == from_y)
        {
            if (to_y < node->y)
                y = astar_map.border_y[node->y];
            else if (to_y > node->y)
                y = astar_map.border_y[node->y+1];
            else
                y = start_y;
        }
        else if (from_y < node->y)
            y = astar_map.border_y[node->y];
        else
            y = astar_map.border_y[node->y+1];

        astar_path.path_x[astar_path.path_count] = x;
        astar_path.path_y[astar_path.path_count] = y;
        ++astar_path.path_count;

        assert(astar_path.path_count <= MAX_PATH_NODE);

        from_x = node->x;
        from_y = node->y;
        start_x = x;
        start_y = y;

        node = node->next;
    }

    return astar_path.path_count != 0;
}

// 最的一次查找到的路径是否先移动X方向
bit Astar_Move_X_First()
{
    assert(astar_path.path);
    assert(astar_path.path->next);

    return astar_path.path->y == astar_path.path->next->y;
}
